#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include "../Moka/utilities.h"
#include "../Moka/cosmology.h"
#include "../Moka/halo.h"
#include "../Moka/nfwHalo.h"
#include "../Moka/nfwLens.h"
#include <gsl/gsl_math.h>
#include <gsl/gsl_spline.h>
#include "nr3.h"
#include "mins.h"
#include "mins_ndim.h"
 
/*
  this code estimate the best fit Mass and Concentration
  fitting the reduced tangential shear and einstein radius from 
  clusters (created with MOKA)
 */

int writeCHI;
int casetocons;
double factor;

double ThetaCUT=5.;

class Func_minwlsl{
  private:
  nfwLens *nl;
  double zl,thetaE;
  int nwhere;
  double m,c;
  std:: vector<double> r,gt,sgt2;
  public: 
  Func_minwlsl(nfwLens *_nl, double _zl, int _nwhere, double _thetaE,
	       std:: vector<double> _r, std:: vector<double> _gt,
	       std:: vector<double> _sgt2)
    : nl(_nl), zl(_zl), nwhere(_nwhere), thetaE(_thetaE), r(_r), gt(_gt), sgt2(_sgt2){}
  double operator() (VecDoub &x){
    if(x[1] < 0 || x[0] < 10 || x[0] > 20 || x[1] > 50){
      return 1.e30;
    }
    int nlp=r.size();
    double chi,rE,rs;
    m = pow(10.,x[0]);
    c = x[1];
    nl->reset(m,zl,c);
    rE = nl->getRe();
    rs = nl->getScaleRadius();
    chi=0;
    for(int l=0;l<nlp;l++) chi += gsl_pow_2(gt[l]-nl->gamma(r[l]/rs)/(1-nl->kappa(r[l]/rs)))/sgt2[l];
    if(writeCHI==1) return chi;
    // 0.1 --- > \sigma_E^2 = (\Delta r)^2/N
    // Einstein Ring to be at least 10 arcsec
    if(casetocons!=3)
      if(thetaE>=ThetaCUT) chi += double(nwhere)*(gsl_pow_2(rE - thetaE)/(factor*factor*0.1));   // weight there up to the bin where the EiR is located    
    if(casetocons==3)
      if(thetaE>=ThetaCUT) chi += (gsl_pow_2(rE - thetaE)/(factor*factor*0.1));
    if(writeCHI==2){
      if(thetaE>=ThetaCUT) return double(nwhere)*gsl_pow_2(rE - thetaE)/(factor*factor*0.1);
      else return 0;
    }
    return chi;
  }
};

class Delta1UPFunc_minwlsl{
  private:
  nfwLens *nl;
  double zl,thetaE;
  int nwhere;
  double m;
  double mmin, cmin,chi2min;
  std:: vector<double> r,gt,sgt2;
  public: 
  Delta1UPFunc_minwlsl(nfwLens *_nl, double _zl, int _nwhere, double _thetaE,
		       std:: vector<double> _r, std:: vector<double> _gt,
		       std:: vector<double> _sgt2, double _mmin, double _cmin, double _chi2min)
    : nl(_nl), zl(_zl), nwhere(_nwhere), thetaE(_thetaE), r(_r), gt(_gt), sgt2(_sgt2),
      mmin(_mmin), cmin(_cmin), chi2min(_chi2min){}
  double operator() (VecDoub &x){
    if(x[0] < mmin || x[0] > 20){
      return 1.e30;
    }
    int nlp=r.size();
    double chi,rE,rs;
    m = pow(10.,x[0]);
    nl->reset(m,zl,cmin);
    rE = nl->getRe();
    rs = nl->getScaleRadius();
    chi=0;
    for(int l=0;l<nlp;l++) chi += gsl_pow_2(gt[l]-nl->gamma(r[l]/rs)/(1-nl->kappa(r[l]/rs)))/sgt2[l];
    // 0.1 --- > \sigma_E^2 = (\Delta r)^2/N
    if(casetocons!=3)    
      if(thetaE>=ThetaCUT) chi += double(nwhere)*(gsl_pow_2(rE - thetaE)/(factor*factor*0.1));
    if(casetocons==3)
      if(thetaE>=ThetaCUT) chi += (gsl_pow_2(rE - thetaE)/(factor*factor*0.1));
    return fabs(chi-chi2min-1);
  }
};

class Delta1DOWNFunc_minwlsl{
  private:
  nfwLens *nl;
  double zl,thetaE;
  int nwhere;
  double m;
  double mmin, cmin,chi2min;
  std:: vector<double> r,gt,sgt2;
  public: 
  Delta1DOWNFunc_minwlsl(nfwLens *_nl, double _zl, int _nwhere, double _thetaE,
			 std:: vector<double> _r, std:: vector<double> _gt,
			 std:: vector<double> _sgt2, double _mmin, double _cmin, double _chi2min)
    : nl(_nl), zl(_zl), nwhere(_nwhere), thetaE(_thetaE), r(_r), gt(_gt), sgt2(_sgt2),
      mmin(_mmin), cmin(_cmin), chi2min(_chi2min){}
  double operator() (VecDoub &x){
    if(x[0] < 10 || x[0] > mmin){
      return 1.e30;
    }
    int nlp=r.size();
    double chi,rE,rs;
    m = pow(10.,x[0]);
    nl->reset(m,zl,cmin);
    rE = nl->getRe();
    rs = nl->getScaleRadius();
    chi=0;
    for(int l=0;l<nlp;l++) chi += gsl_pow_2(gt[l]-nl->gamma(r[l]/rs)/(1-nl->kappa(r[l]/rs)))/sgt2[l];
    // 0.1 --- > \sigma_E^2 = (\Delta r)^2/N
    if(casetocons!=3)
      if(thetaE>=ThetaCUT) chi += double(nwhere)*(gsl_pow_2(rE - thetaE)/(factor*factor*0.1));
    if(casetocons==3)
      if(thetaE>=ThetaCUT) chi += (gsl_pow_2(rE - thetaE)/(factor*factor*0.1));
    return fabs(chi-chi2min-1);
  }
};

class Delta2UPFunc_minwlsl{
  private:
  nfwLens *nl;
  double zl,thetaE;
  int nwhere;
  double m;
  double mmin, cmin,chi2min;
  std:: vector<double> r,gt,sgt2;
  public: 
  Delta2UPFunc_minwlsl(nfwLens *_nl, double _zl, int _nwhere, double _thetaE,
		       std:: vector<double> _r, std:: vector<double> _gt,
		       std:: vector<double> _sgt2, double _mmin, double _cmin, double _chi2min)
    : nl(_nl), zl(_zl), nwhere(_nwhere), thetaE(_thetaE), r(_r), gt(_gt), sgt2(_sgt2),
      mmin(_mmin), cmin(_cmin), chi2min(_chi2min){}
  double operator() (VecDoub &x){
    if(x[0] < cmin || x[0] > 50){
      return 1.e30;
    }
    int nlp=r.size();
    double chi,rE,rs;
    m = pow(10.,mmin);
    nl->reset(m,zl,x[0]);
    rE = nl->getRe();
    rs = nl->getScaleRadius();
    chi=0;
    for(int l=0;l<nlp;l++) chi += gsl_pow_2(gt[l]-nl->gamma(r[l]/rs)/(1-nl->kappa(r[l]/rs)))/sgt2[l];
    // 0.1 --- > \sigma_E^2 = (\Delta r)^2/N
    if(casetocons!=3)
      if(thetaE>=ThetaCUT) chi += double(nwhere)*(gsl_pow_2(rE - thetaE)/(factor*factor*0.1));
    if(casetocons==3)
      if(thetaE>=ThetaCUT) chi += (gsl_pow_2(rE - thetaE)/(factor*factor*0.1));
    return fabs(chi-chi2min-1);
  }
};

class Delta2DOWNFunc_minwlsl{
  private:
  nfwLens *nl;
  double zl,thetaE;
  int nwhere;
  double m;
  double mmin, cmin,chi2min;
  std:: vector<double> r,gt,sgt2;
public: 
  Delta2DOWNFunc_minwlsl(nfwLens *_nl, double _zl, int _nwhere, double _thetaE,
			 std:: vector<double> _r, std:: vector<double> _gt,
			 std:: vector<double> _sgt2, double _mmin, double _cmin, double _chi2min)
    : nl(_nl), zl(_zl), nwhere(_nwhere), thetaE(_thetaE), r(_r), gt(_gt), sgt2(_sgt2),
      mmin(_mmin), cmin(_cmin), chi2min(_chi2min){}
  double operator() (VecDoub &x){
    if(x[0] < 0 || x[0] > cmin){
      return 1.e30;
    }
    int nlp=r.size();
    double chi,rE,rs;
    m = pow(10.,mmin);
    nl->reset(m,zl,x[0]);
    rE = nl->getRe();
    rs = nl->getScaleRadius();
    chi=0;
    for(int l=0;l<nlp;l++) chi += gsl_pow_2(gt[l]-nl->gamma(r[l]/rs)/(1-nl->kappa(r[l]/rs)))/sgt2[l];
    // 0.1 --- > \sigma_E^2 = (\Delta r)^2/N
    if(casetocons!=3)    
      if(thetaE>=ThetaCUT) chi += double(nwhere)*(gsl_pow_2(rE - thetaE)/(factor*factor*0.1));
    if(casetocons==3)
      if(thetaE>=ThetaCUT) chi += (gsl_pow_2(rE - thetaE)/(factor*factor*0.1));
    return fabs(chi-chi2min-1);
  }
};

class Func_minkappa{
  private:
  nfwLens *nl;
  double zl;
  double m,c;  
  std:: vector<double> r,kappa,sk2;
  public: 
  Func_minkappa(nfwLens *_nl, double _zl, std:: vector<double> _r, std:: vector<double> _kappa,
	       std:: vector<double> _sk2)
    : nl(_nl), zl(_zl), r(_r), kappa(_kappa), sk2(_sk2){}
  double operator() (VecDoub &x){
    if(x[1] < 0 || x[0] < 10 || x[0] > 20 || x[1] > 50 ){
      return 1.e30;
    }
    int nlp=r.size();
    double chi,rs;
    m = pow(10.,x[0]);
    c = x[1];
    nl->reset(m,zl,c);
    rs = nl->getScaleRadius();
    chi=0;
    for(int l=0;l<nlp;l++){
      if(sk2[l]>0) chi += gsl_pow_2(kappa[l]-nl->kappa(r[l]/rs))/sk2[l];
    }
    return chi;
  }
};

int main(){
  double but,thetaEcheck;
  int ih,ibut;

  double observational,intrinsic;

  std:: cout << " halo id where to start from " << std:: endl; 
  std:: cin >> ih;
  std:: cout << " case to consider " << std:: endl;
  std:: cout << " 1. WL + SL (with weight) " << std:: endl;
  std:: cout << " 2. WL                    " << std:: endl;
  std:: cout << " 3. WL (outside rE) + SL (no weight) " << std:: endl;
  std:: cout << " 4. WL (outside rE) + SL (with weight) " << std:: endl;
  std:: cin >> casetocons;
  int cwl;
  std:: cin >> cwl;
  std:: cin >> factor;
  std:: cin >> writeCHI;
  std:: cin >> observational;
  std:: cin >> intrinsic;
  std:: string scase,sfactor,sobs,sint;
  std:: ostringstream oscase,osfactor,oobs,oint;
  oscase << casetocons;
  scase = oscase.str();
  osfactor << int(factor);
  sfactor = osfactor.str();
  oobs << int(observational);
  sobs = oobs.str();
  oint << int(intrinsic);
  sint = oint.str();
  std:: string sThetaCUT;
  std:: ostringstream osThetaCUT;
  osThetaCUT << int(ThetaCUT);
  sThetaCUT = osThetaCUT.str();

  double largestgt=1.e+10;
  if(cwl==1) largestgt=0.1;
  std:: string filin;
  std:: ifstream streamfilin;
  std:: ifstream streamINPUT;
  streamINPUT.open("INPUT");
  double om0,oml,h,w,zl,zs,fov;
  int npix;
  if(streamINPUT.is_open()){
    std:: string str;
    double butr;
    string mtrue0;
    string nh0;
    streamINPUT >> str;
    streamINPUT >> om0;
    streamINPUT >> str; 
    streamINPUT >> oml;
    streamINPUT >> str;
    streamINPUT >> h;
    streamINPUT >> str;
    streamINPUT >> w;
    streamINPUT >> str;
    streamINPUT >> zl;
    streamINPUT >> str;
    streamINPUT >> mtrue0;
    streamINPUT >> str;
    streamINPUT >> zs;
    streamINPUT >> str;
    streamINPUT >> fov;
    streamINPUT >> str;
    streamINPUT >> str;
    streamINPUT >> str;
    streamINPUT >> nh0;
    streamINPUT >> str;
    streamINPUT >> npix;
    // streamINPUT >> str;
    // streamINPUT >> str;
    // streamINPUT >> str;
    // streamINPUT >> str;
    // streamINPUT >> str;
    // streamINPUT >> filmcrel;
    /*
      streamfilmcrel.open(filmcrel.c_str());
      if(streamfilmcrel.is_open()){}
      else{
      std:: cout << filmcrel << " not exist!" << std:: endl;
      std:: cout << "  I will STOP here ... check it out! " << std:: endl;
      exit(1);
      }
    */
  }
  else{
    std:: cout << "INPUT not exist!" << std:: endl;
    std:: cout << "  I will STOP here ... check it out! " << std:: endl;
    exit(1);    
  }
  // cosmological background
  cosmology co(om0,oml,h,w);
  std:: string fileinfo="info_haloes.dat";
  std:: ifstream streamfileinfo;
  streamfileinfo.open(fileinfo.c_str());
  double mtrue,ctrue,thetaE;
  std:: vector<double> r,gt,A,sgt2,sgit2,theta,kappa,skappa2;
  std:: string mfilout = "estinfo_haloes_case" + scase + "_" + sfactor + ".dat" + sThetaCUT + "int" + sint + "obs" + sobs;
  if(cwl==1) mfilout = "estinfo_haloes_case" + scase + "_" + sfactor + "WL.dat" + sThetaCUT + "int" + sint + "obs" + sobs;
  std:: ofstream mstreamfilout;
  mstreamfilout.open(mfilout.c_str());
  // for each halo which is in info_haloes.dat
  while(streamfileinfo >> mtrue >> ctrue >> zl >> 
	but >> but >> but >> but >> but >> but >> but >> ibut >> but >> but >> 
	but >> but >> but >> but >> but >> thetaE >> but >> but >> but >> but >> 
	// but >> but >> but >> but >> but >> thetaE >> but >> thetaE >> but >> but >> 
	but >> but >> but >> but >> but >> but >> but >> but >> but >> but >> 
	but >> but >> but >> but >> but >> but >> but >> but >> but >> but){
    // only weak lensing info is used in this case
    if(casetocons==2) thetaE=0;
    // only sheart above this value is used
    thetaEcheck = thetaE;
    if(mtrue<100.){
      std:: cout << " Mtrue = " << mtrue << "  strange value for halo mass " << std:: endl;
      std:: cout << "     check it out ... I will STOP here!!! " << std:: endl;
      exit(1);
    }
    std:: cout << "halo " << ih << "   " << mtrue << "  " << ctrue << " " << zl << "  " << thetaE << std:: endl;
    // read the gt profile
    std:: string sid,snpix;
    std:: ostringstream osid,osnpix;
    osid << ih;
    sid = osid.str();
    osnpix << npix;
    snpix = osnpix.str();
    filin = "fits/"+sid+"MAP_radial_prof"+snpix+".dat";
    std:: cout << "  reading " << filin<< std:: endl;
    streamfilin.open(filin.c_str());
    if(streamfilin.is_open()){
    }
    else{
      std:: cout << filin << " not exist!" << std:: endl;
      std:: cout << "  I will STOP here ... check it out! " << std:: endl;
      exit(1);
    }  
    // all the shear profile is used in these cases
    if(casetocons<3) thetaEcheck = 0.;
    std:: string sbut;
    for(int i=0;i<14;i++) streamfilin >> sbut;
    double br,bgt,bsgit,bA,btheta,bkappa,bskappa;
    r.clear();
    gt.clear();
    A.clear();
    sgt2.clear();
    sgit2.clear();
    theta.clear();
    kappa.clear();
    skappa2.clear();
    double ngal = 30.; // number of background galaxies
    while(streamfilin >> br >> bkappa >> bskappa >> but >> but >> bgt >> 
	  bsgit >> but >> but >> but >> but >> btheta >> bA){
      // it consider the profile only for these conditions
      if(bgt>0 && bgt<=largestgt && btheta>=thetaEcheck){
	if(observational == 0 && intrinsic == 1){
	  if(bsgit>0){
	    r.push_back(br);
	    gt.push_back(bgt);
	    A.push_back(bA);
	    sgt2.push_back( observational*0.09/(bA/60./60.*ngal) + intrinsic*bsgit*bsgit);
	    sgit2.push_back(bsgit*bsgit); // intrinsic dispersion
	    theta.push_back(btheta);
	    kappa.push_back(bkappa);
	    skappa2.push_back(bskappa*bskappa);
	  }
	}else{
	  r.push_back(br);
	  gt.push_back(bgt);
	  A.push_back(bA);
	  sgt2.push_back( observational*0.09/(bA/60./60.*ngal) + intrinsic*bsgit*bsgit);
	  sgit2.push_back(bsgit*bsgit); // intrinsic dispersion
	  theta.push_back(btheta);
	  kappa.push_back(bkappa);
	  skappa2.push_back(bskappa*bskappa);
	}
      }
    }
    streamfilin.close();
    int nlp=r.size();
    int nwhere=1;
    if(thetaE>theta[0]){
      nwhere = locate(theta,thetaE);
      //std:: cout << nwhere << std:: endl;      
      nwhere=GSL_MIN( GSL_MAX( nwhere, 0 ), nlp-1 )+2;
      std:: cout << nwhere << std:: endl;
    }
    halo ha(&co,mtrue,zl);
    nfwHalo nh(ha,ctrue);
    nfwLens nl(nh,zs);

    double mest,cest;
    VecDoub XX(2);
    XX[0] = log10(mtrue);
    XX[1] = ctrue;
    Func_minwlsl func (&nl,zl,nwhere,thetaE,r,gt,sgt2);
    double truewlsl = func(XX);
    Powell<Func_minwlsl> powell(func,1.e-2);
    XX = powell.minimize(XX);
    double minwlsl = powell.fret;

    VecDoub XX1(1);
    XX1[0] = XX[0];
    Delta1UPFunc_minwlsl funcD1UP (&nl,zl,nwhere,thetaE,r,gt,sgt2,XX[0],XX[1],minwlsl);
    Powell<Delta1UPFunc_minwlsl> powellD1UP(funcD1UP,1.e-2);
    XX1 = powellD1UP.minimize(XX1);

    VecDoub XX2(1);
    XX2[0] = XX[0];
    Delta1DOWNFunc_minwlsl funcD1DOWN (&nl,zl,nwhere,thetaE,r,gt,sgt2,XX[0],XX[1],minwlsl);
    Powell<Delta1DOWNFunc_minwlsl> powellD1DOWN(funcD1DOWN,1.e-2);
    XX2 = powellD1DOWN.minimize(XX2);

    VecDoub YY1(1);
    YY1[0] = XX[1];
    Delta2UPFunc_minwlsl funcD2UP (&nl,zl,nwhere,thetaE,r,gt,sgt2,XX[0],XX[1],minwlsl);
    Powell<Delta2UPFunc_minwlsl> powellD2UP(funcD2UP,1.e-2);
    YY1 = powellD2UP.minimize(YY1);

    VecDoub YY2(1);
    YY2[0] = XX[1];
    Delta2DOWNFunc_minwlsl funcD2DOWN (&nl,zl,nwhere,thetaE,r,gt,sgt2,XX[0],XX[1],minwlsl);
    Powell<Delta2DOWNFunc_minwlsl> powellD2DOWN(funcD2DOWN,1.e-2);
    YY2 = powellD2DOWN.minimize(YY2);


    double sigmaM = 0.5*(XX[0] - XX2[0])*(XX1[0] - XX[0]);
    double sigmaC = 0.5*(XX[1] - YY2[0])*(YY1[0] - XX[1]);

    VecDoub YY(2);
    YY[0] = log10(mtrue);
    YY[1] = ctrue;

    Func_minkappa func2 (&nl,zl,r,kappa,skappa2);
    double truekappa = func(YY);
    Powell<Func_minkappa> powell2(func2,1.e-2);
    YY = powell2.minimize(YY);
    double minkappa = powell2.fret;
    // mass and concentration estimate from SL + WL
    mest = pow(10.,XX[0]);
    cest = XX[1];
    nl.reset(mest,zl,cest);
    double thetaE2 =  nl.getRe();
    mstreamfilout << mest << "   "  << (2.3*mest*sigmaM) << "  " << cest << "   "  << sigmaC << "  "  << thetaE2 << "   " 
		  << pow(10.,YY[0]) << "   " << YY[1] 
		  << "   " << minwlsl << "   " << minkappa 
		  << "   " << truewlsl << "   " << truekappa 
		  << std:: endl;
    double rs=nl.getScaleRadius();
    std:: string filout = "fits/gtprofFromEstMandC"+sid+"_case"+scase+"_" + sfactor + ".dat" + sThetaCUT + "int" + sint + "obs" + sobs;
    if(cwl==1) filout = "fits/gtprofFromEstMandC"+sid+"_case"+scase+"_" + sfactor + "WL.dat" + sThetaCUT + "int" + sint + "obs" + sobs;
    std:: ofstream streamfilout;
    streamfilout.open(filout.c_str());
    for(int l=0;l<nlp;l++){
      streamfilout << nl.gamma(r[l]/rs) << "   " << nl.gamma(r[l]/rs)/(1-nl.kappa(r[l]/rs)) << "  " << r[l] << std:: endl;
    }
    streamfilout.close();
    ih++;
    std:: cout << "   " << std::endl;
    
    if(writeCHI>0){
      VecDoub CC(2);
      ofstream myfile,myfile2;
      if(writeCHI==1){myfile.open ("dataWL.txt");}
      if(writeCHI==2){myfile.open ("dataSL.txt");}
      if(writeCHI==3){myfile.open ("dataWL+SL.txt");}            
      if(writeCHI==1){myfile2.open ("chidataWL.txt");}
      if(writeCHI==2){myfile2.open ("chidataSL.txt");}
      if(writeCHI==3){myfile2.open ("chidataWL+SL.txt");}            
      int npix = 256;
      std:: vector<double> lm,lc;
      fill_linear(lm,npix,log10(mtrue)-0.25,log10(mtrue)+0.25);
      fill_linear(lc,npix,log10(ctrue)-0.5,log10(ctrue)+0.5);
      for(int ki=0;ki<npix;ki++){
          CC[0] = lm[ki];
        for(int kj=0;kj<npix;kj++){ 
          CC[1] = pow(10.,lc[kj]);
          double Xk = func(CC);
          myfile << lm[ki] << "   " << lc[kj] << "  " << log10(Xk) << std:: endl;    
	  myfile2 << Xk << std:: endl;
        }
        myfile << "  " << std:: endl;     
      }    
      exit(1);    
    }
  }
  streamfileinfo.close();  
  mstreamfilout.close();
}
